#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include "analysis.h"

/*
 * radian convertion angle.
 */
float radian_to_angle(float *ft)
{
	return *ft * (180/M_PI);
}

/*
 * angle convertion radian.
 */
float angle_to_radian(float *ft)
{
	return *ft * (M_PI/180);
}

static float imu_data_to_float( const unsigned char data_h, const unsigned char data_l)
{
	union 
	{
		short data_short;
		unsigned short data_ushort;
	} us2s;

	us2s.data_ushort = (((unsigned short)data_h) << 8) | data_l;

	return (float)us2s.data_short;
}

static float imu_data_to_float2( const unsigned short data)
{
	union 
	{
		short data_short;
		unsigned short data_ushort;
	} us2s;

	us2s.data_ushort = ((unsigned short)data);

	return (float)us2s.data_short;
}

void imu_data_resolving(struct icm42686_data * src, struct imu_acc * acc, struct imu_gry * gry)
{
	acc->ax = (imu_data_to_float(src->acc.x>>8 & 0xff, src->acc.x & 0xff)) * 4 / 65536.0;
	acc->ay = (imu_data_to_float(src->acc.y>>8 & 0xff, src->acc.y & 0xff)) * 4 / 65536.0;
	acc->az = (imu_data_to_float(src->acc.z>>8 & 0xff, src->acc.z & 0xff)) * 4 / 65536.0;

	gry->gx = (imu_data_to_float(src->gyro.x>>8 & 0xff, src->gyro.x & 0xff)) * 2000 / 65536.0;
	gry->gy = (imu_data_to_float(src->gyro.y>>8 & 0xff, src->gyro.y & 0xff)) * 2000 / 65536.0;
	gry->gz = (imu_data_to_float(src->gyro.z>>8 & 0xff, src->gyro.z & 0xff)) * 2000 / 65536.0;

    printf("%f, %f, %f, %f, %f, %f\n", angle_to_radian(&gry->gx), angle_to_radian(&gry->gy), angle_to_radian(&gry->gz),
                                         acc->ax, acc->ay, acc->az);
}

int imu_data_resolving2(struct icm42686_data * src, struct imu_acc * acc, struct imu_gry * gry, unsigned long long time)
{
	acc->ax = (imu_data_to_float(src->acc.x>>8 & 0xff, src->acc.x & 0xff)) * 4 / 65536.0;
	acc->ay = (imu_data_to_float(src->acc.y>>8 & 0xff, src->acc.y & 0xff)) * 4 / 65536.0;
	acc->az = (imu_data_to_float(src->acc.z>>8 & 0xff, src->acc.z & 0xff)) * 4 / 65536.0;

	gry->gx = (imu_data_to_float(src->gyro.x>>8 & 0xff, src->gyro.x & 0xff)) * 2000 / 65536.0;
	gry->gy = (imu_data_to_float(src->gyro.y>>8 & 0xff, src->gyro.y & 0xff)) * 2000 / 65536.0;
	gry->gz = (imu_data_to_float(src->gyro.z>>8 & 0xff, src->gyro.z & 0xff)) * 2000 / 65536.0;

    printf("%d, %f, %f, %f, %f, %f, %f\n", time, angle_to_radian(&gry->gx), angle_to_radian(&gry->gy), angle_to_radian(&gry->gz),
                                         acc->ax, acc->ay, acc->az);
    return 0;
}

int analysis(char *pdata)
{
    struct imu_acc acc;
    struct imu_gry gry;
    struct icm42686_data imu = {0};

    /* 0x0900, 0xcc20, 0xd8d0, 0xffef, 0xfff1, 0xffec */
    sscanf(pdata, "%04x, %04x, %04x, %04x, %04x, %04x\n", &imu.acc.x, &imu.acc.y, &imu.acc.z, 
                                            &imu.gyro.x, &imu.gyro.y, &imu.gyro.z);

    printf("anas:0x%04x, 0x%04x, 0x%04x, 0x%04x, 0x%04x, 0x%04x\n",
                                            imu.acc.x, imu.acc.y, imu.acc.z, 
                                            imu.gyro.x, imu.gyro.y, imu.gyro.z);

    imu_data_resolving(&imu, &acc, &gry);

    return 0;
}

int analysis_float(char *pdata)
{
    struct imu_acc acc;
    struct imu_gry gry;
    struct icm42686_data imu = {0};
    float f;

    /* [  329.158856] 0x0900, 0xcc20, 0xd8d0, 0xffef, 0xfff1, 0xffec */
    sscanf(pdata, "[  %f] %x, %x, %x, %x, %x, %x", &f, &imu.acc.x, &imu.acc.y, &imu.acc.z, 
                                            &imu.gyro.x, &imu.gyro.y, &imu.gyro.z);

    /*printf("anas:%f, 0x%04x, 0x%04x, 0x%04x, 0x%04x, 0x%04x, 0x%04x\n", f,
                                            imu.acc.x, imu.acc.y, imu.acc.z, 
                                            imu.gyro.x, imu.gyro.y, imu.gyro.z);*/

    imu_data_resolving(&imu, &acc, &gry);

    return 0;
}

int analysis_float_time(char *pdata)
{
    struct imu_acc acc;
    struct imu_gry gry;
    struct icm42686_data imu = {0};
    float f;
    char c = 0;
    int time = 0; 

    /* [   72.367499] f 72363, 0xe2c0, 0xfe40, 0x37e4, 0xfffc, 0x0001, 0x0003 */
    sscanf(pdata, "[   %f] %c %d, %x, %x, %x, %x, %x, %x", 
                                            &f, &c, &time, 
                                            &imu.acc.x, &imu.acc.y, &imu.acc.z, 
                                            &imu.gyro.x, &imu.gyro.y, &imu.gyro.z);

    /*printf("anas:%d, 0x%04x, 0x%04x, 0x%04x, 0x%04x, 0x%04x, 0x%04x\n", time,
                                            imu.acc.x, imu.acc.y, imu.acc.z, 
                                            imu.gyro.x, imu.gyro.y, imu.gyro.z);*/

    imu_data_resolving2(&imu, &acc, &gry, time);

    return 0;
}

int main(int argc, char **argv)
{
    char *file_name = NULL;
    FILE *fp = NULL;
    char data[1024] = {0};
    int i = 0;

    file_name = argv[1];
    //printf("file name:%s\n", file_name);

    fp = fopen(file_name, "r");
    if (fp == NULL) {
        printf("file open fail.\n");
        exit(-1);
    }

    while (!feof(fp)) {
        fgets(data, 1024, fp);
        //printf("%s", data);
        //analysis(data);
        //analysis_float(data);
        analysis_float_time(data);
    }

    fclose(fp);

    printf("\n");

    return 0;
}

